using System;
using System.Collections.Generic;
using System.Text;

namespace ImagesMath.Transformations
{
    public class Rotation : Transformation
    {


        private VR3 axis;
        private double angle;

        public Rotation(VR3 axis,double angle) : base(new M33(),axis)
        {
            this.angle = angle;
            this.axis = axis;
            Update();
        }

        //Rotation::Rotation(const vR3& _Axis,double _Angle):Axis(_Axis),Angle(_Angle)
        //{
        //  update();
        //}

        public VR3    getaxis()
        {
            return axis;
        }
        public double getAngle()
        {
            return angle;
        }
        public void   setaxis(ref VR3 axis)
        {
            this.axis = axis;
            Update();
        }

        public void   setAngle(double angle)
        {
            this.angle = angle;
            Update();
        }



        /// Compute the concatenation of (*this) and Tr [ (*this)*Tr ]
        public override Transformation rightMultiply(Transformation Tr)
        {
            return new Transformation(L * Tr.L, L * Tr.T, Tr.InvL * invL, Tr.InvT);
            
        }
        /// Compute the concatenation of Tr and (*this) [ Tr*(*this) ]
        public override Transformation leftMultiply(Transformation Tr)
        {
            return new Transformation(Tr.L*L, Tr.T, invL*Tr.InvL, invL*Tr.InvT);
        }
        public override PR3 transform(PR3 p)
        {
            return L * p;
        }
        public override PR3 invTransform(PR3 p)
        {
            return invL * p;
        }
        public override VR3 transform(VR3 v)
        {
            return L * v;
        }
        public override VR3 invTransform(VR3 v)
        {
            return invL * v;
        }


        public void Update()
        {

            double cosa = Math.Cos(angle);
            double sina = Math.Sin(angle);

            L = new M33();
            double xx,xy,xz,yx,yy,yz,zx,zy,zz;

            xx = cosa + axis.X * axis.X * (1 - cosa);
            xy = axis.X * axis.Y * (1 - cosa) + axis.Z * sina;
            xz = axis.X * axis.Z * (1 - cosa) - axis.Y * sina;

            yx = axis.X * axis.Y * (1 - cosa) - axis.Z * sina;
            yy = cosa + axis.Y * axis.Y * (1 - cosa);
            yz = axis.Y * axis.Z * (1 - cosa) + axis.X * sina;

            zx = axis.X * axis.Z * (1 - cosa) + axis.Y * sina;
            zy = axis.Y * axis.Z * (1 - cosa) - axis.X * sina;
            zz = cosa + axis.Z * axis.Z * (1 - cosa);

            VR3 vx = new VR3(xx,xy,xz);
            VR3 vy = new VR3(yx, yy, yz);
            VR3 vz = new VR3(zx, zy, zz);

            L.VX = vx;
            L.VY = vy;
            L.VZ = vz;
            

            //L.VX.X = cosa + axis.X * axis.X * (1 - cosa);
            //L.VX.Y = axis.X * axis.Y * (1 - cosa) + axis.Z * sina;
            //L.VX.Z = axis.X * axis.Z * (1 - cosa) - axis.Y * sina;

            //L.VY.X = axis.X * axis.Y * (1 - cosa) - axis.Z * sina;
            //L.VY.Y = cosa + axis.Y * axis.Y * (1 - cosa);
            //L.VY.Z = axis.Y * axis.Z * (1 - cosa) + axis.X * sina;

            //L.VZ.X = axis.X * axis.Z * (1 - cosa) + axis.Y * sina;
            //L.VZ.Y = axis.Y * axis.Z * (1 - cosa) - axis.X * sina;
            //L.VZ.Z = cosa + axis.Z * axis.Z * (1 - cosa);

            invL = L.transpose();
          
        }

        //  class Rotation : public Transform
        //  {
        //    vR3    axis;
        //    double Angle;

        ///// Update M and invM according to axis or angle modification.
        //    void update();

        //    public:

        //      Rotation(const vR3& _axis,double _Angle);

        //      vR3    getaxis()  const;
        //      double getAngle() const;
        //      void   setaxis(const vR3& _axis);
        //      void   setAngle(double _Angle);

        ///// Compute the concatenation of (*this) and Tr [ (*this)*Tr ]
        //      Transform rightMultiply(const Transform& Tr) const;
        ///// Compute the concatenation of Tr and (*this) [ Tr*(*this) ]
        //      Transform leftMultiply (const Transform& Tr) const;
        //      pR3       transform    (const pR3& p) const;
        //      pR3       invTransform (const pR3& p) const;
        //      vR3       transform    (const vR3& v) const;
        //      vR3       invTransform (const vR3& v) const;
        //  };
        //
// Rotation interface
//


//Rotation::Rotation(const vR3& _axis,double _Angle):axis(_axis),Angle(_Angle)
//{
//  update();
//}


//vR3 Rotation::getaxis() const
//{
//  return axis;
//}


//double Rotation::getAngle() const
//{
//  return Angle;
//}


//void Rotation::setaxis(const vR3& _axis)
//{
//  axis = normalize(_axis);
//  update();
//}


//void Rotation::setAngle(double _Angle)
//{
//  Angle = _Angle;
//  update();
//}


//Transform Rotation::rightMultiply(const Transform& Tr) const
//{
//  return Transform(L*Tr.L, L*Tr.T, Tr.invL*invL, Tr.invT);
//}


//Transform Rotation::leftMultiply(const Transform& Tr) const
//{
//  return Transform(Tr.L*L, Tr.T, invL*Tr.invL, invL*Tr.invT);
//}


//pR3 Rotation::transform(const pR3& p) const
//{
//  return L*p;
//}


//pR3 Rotation::invTransform(const pR3& p) const
//{
//  return invL*p;
//}


//vR3 Rotation::transform(const vR3& v) const
//{
//  return L*v;
//}


//vR3 Rotation::invTransform(const vR3& v) const
//{
//  return invL*v;
//}

    }
}
